package roborally;

import gui.IRobot;

import java.util.HashSet;
import java.util.Set;

import be.kuleuven.cs.som.annotate.Basic;
import be.kuleuven.cs.som.annotate.Immutable;
import be.kuleuven.cs.som.annotate.Model;
import be.kuleuven.cs.som.annotate.Raw;

/**
 * @invar The robot must have a valid energy level at all times.
 * 		| canHaveAsEnergyLevel(this.getEnergyLevel())
 * @invar The robot must have a valid x position at all times.
 * 		| canHaveAsX(this.getX())
 * @invar The robot must have a valid y position at all times.
 * 		| canHaveAsY(this.getY())
 * @invar The robot must have a valid orientation at all times.
 * 		| canHaveAsOrientation(this.getOrientation())
 * 
 * @author Molderez Tom, 2de bach Ingenieurswetenschappen Computerwetenschappen-Elektrotechniek
 * @author and Reniers Jorn, 2de bach Ingenieurswetenschappen Werktuigkunde-Computerwetenschappen
 * @version 5.0
 *
 */
public class Robot implements IRobot {
	
//constructors -----------------------------------------------------------------------------------------------
	/**
	 * Constructs a new Robot with the given options.
	 * 
	 * @param x The x position of the robot.
	 * @param y The y position of the robot.
	 * @param orientation The orientation of the robot.
	 * @param energyLevel The energyLevel of the robot.
	 * @param energyLimit The energyLimit of the robot.
	 * @pre The energyLimit must be greater or equal then 0.
	 * 		| energyLimit >= 0
	 * @effect The x position is set to the given x position.
	 * 		| setX(x) 
	 * @effect The y position is set to the given y position.
	 * 		| setY(y)
	 * @effect The orientation is set to the given orientation.
	 * 		| setOrientation(orientation)
	 * @effect The EnergyLevel of the robot is set to the given energyLevel
	 * 		| setEnergyLevel(energyLevel)
	 * @post The energyLimit of the robot is set to the given energyLimit
	 * 		| (new this).getEnergyLimit() == energyLimit
	 * 			
	 * @throws IllegalArgumentException
	 * 		The given x position is not a valid one.
	 * 		| !canHaveAsX(x)
	 * @throws IllegalArgumentException
	 * 		The given y position is not a valid one.
	 * 		| !canHaveAs(y)
	 */
	@Raw
	public Robot(long x, long y, int orientation, double energyLevel, double energyLimit ) throws IllegalArgumentException {
		setX(x);
		setY(y);
		setOrientation(orientation);
		
		this.ENERGY_LIMIT = energyLimit;
		setEnergyLevel(energyLevel);
	}
	
	/**
	 * Constructs a new Robot with the given options and a default energy limit.
	 * 
	 * @param x The x position of the robot.
	 * @param y The y position of the robot.
	 * @param orientation The orientation of the robot.
	 * @param energyLevel The energyLevel of the robot.
	 * @effect Creates a new robot with the default energy limit.
	 * 		| this(x,y,orientation,energyLevel,Robot.DEFAULT_ENERGY_LIMIT)
	 */
	@Raw
	public Robot(long x, long y, int orientation, double energyLevel) throws IllegalArgumentException {
		this(x,y,orientation,energyLevel, Robot.DEFAULT_ENERGY_LIMIT);
	}
	
	/**
	 * Creates a new Robot with the given options and a default maximum energyLevel.
	 * 
	 * @param x The x position of the robot.
	 * @param y The y position of the robot.
	 * @param orientation The orientation of the robot
	 * @effect Creates a new Robot with maximum amount of energy.
	 * 		| this(x,y,orientation, getEnergyLimit())
	 */
	@Raw
	public Robot(long x, long y, int orientation) throws IllegalArgumentException {
		this(x,y,orientation, Robot.DEFAULT_ENERGY_LIMIT);
	}
	
	/**
	 * Creates a new Robot with the given option and a default position and orientation.
	 * 
	 * @param energyLevel The energyLevel of the robot.
	 * @effect Creates a new robot, localized at the origin and UP orientated.
	 * 		| this(0,0,Robot.UP,energyLevel)
	 */
	@Raw
	public Robot(double energyLevel) {
		this(0,0,Robot.UP, energyLevel);
	}
	
	/**
	 * Creates a default Robot.
	 * 
	 * @effect Creates a new robot, localized at the origin, UP orientated and with maximum energy given the default energy limit.
	 * 		| this(Robot.DEFAULT_ENERGY_LIMIT)
	 */
	@Raw
	public Robot() {
		this(Robot.DEFAULT_ENERGY_LIMIT);
	}
	
	
// Destructor ------------------------------------------------------------------------------------------
	
	/**
	 * Terminates this Robot.
	 *
	 * @post This Robot is terminated.
	 *		| (this new).isTerminated()
	 *
	 */
	@Raw
	public void terminate() {
		this.isTerminated = true;
	}

	/**
	 * Checks whether this Robot is terminated.
	 */
	@Basic @Raw
	public boolean isTerminated() {
		return this.isTerminated;
	}

	/**
	 * Variable registering whether this Robot. is terminated.
	 */
	private boolean isTerminated;
			

	
//orientation: total -----------------------------------------------------------------------------------
	
	/**
	 * Returns the orientation of the robot.
	 * 
	 */
	@Basic @Raw
	public int getOrientation() {
		return this.orientation;
	}

	/**
	 * Sets the orientation to the given value. 
	 *
	 * @param orientation The orientation to be set.
	 * @post The orientation is the absolute value of modulo 4 of the given number.
	 * 		| (new this).getOrientation() == Math.abs(orientation % 4)
	 */
	@Raw
	public void setOrientation(int orientation) {
		this.orientation = Math.abs(orientation % 4);
	}
	
	/**
	 * Controls if the orientation is valid.
	 * 
	 * @return result == (this.getOrientation()>0 && this.getOrientation()<4)
	 */
	@Raw
	public boolean canHaveAsOrientation(int orientation){
		return (orientation>=0 && orientation<4);
	}
	
	/**
	 * The orientation of the robot, symbolized by a number.
	 */
	private int orientation;
	
	/**
	 * The up orientation.
	 */
	public static final int UP = 0;
	/**
	 * The right orientation.
	 */
	public static final int RIGHT = 1;
	/**
	 * The down orientation
	 */
	public static final int DOWN = 2;
	/**
	 * The left orientation
	 */
	public static final int LEFT = 3;
	
	
//position: defensively ----------------------------------------------------------------------------------------
	
	/**
	 * Returns the x position.
	 */
	@Basic @Raw
	public long getX() {
		return this.x;
	}
	
	/**
	 * Sets the x position to a given value without energy restrictions
	 * 
	 * @param x The x position to be set.
	 * @post The x position is set to the given value.
	 * 		| (new this).getX() == x
	 * @throws IllegalArgumentException
	 * 		The robot can't have the given x position.
	 * 		| !canHaveAsX(x)
	 * @note This method can be private because it is used when you move a robot, which is done by move.
	 */
	@Raw
	public void setX(long x) throws IllegalArgumentException {
		if(!canHaveAsX(x))
			throw new IllegalArgumentException("x-value is not valid");
		this.x = x;
	}
		
	/**
	 * Controls if the x is a valid xPosition.
	 * 
	 * @param x The x to be controlled
	 * @return true if and only if x is positive and x is smaller then the board limit
	 * 		| result == (x >= 0 && x <= getXLimit())
	 */
	@Raw
	public boolean canHaveAsX(long x) {
		return (x >= 0 && x <= getXLimit());
	}
	
	/**
	 * 
	 * returns the maximum x-value (the edge of the board), Long.MAX_VALUE.
	 */
	@Basic @Raw @Immutable
	public static long getXLimit(){
		return X_BOARD_LIMIT;
	}
		
	//The x position of the robot.
	private long x;
	
	//The x position limit of the robot.
	private static final long X_BOARD_LIMIT = Long.MAX_VALUE; 
	
	/**
	 * Returns the y position
	 * 
	 */
	@Basic @Raw
	public long getY() {
		return this.y;
	}
	
	/**
	 * Sets the y position to a given value without energy restrictions
	 * 
	 * @param y The y position to be set.
	 * @post The y position is set to the given value.
	 * 		| (new this).getY() == y
	 * @throws IllegalArgumentException
	 * 		The robot can't have the given y position.
	 * 		| !canHaveAsY(y)
	 * @note This method can be private because it is used when you move a robot, which is done by move.
	 */
	@Raw
	public void setY(long y) throws IllegalArgumentException {
		if(!canHaveAsY(y))
			throw new IllegalArgumentException("y-value is not valid");
		this.y = y;
	}
	
	/**
	 * Controls if the y is a valid yPosition.
	 * 
	 * @param y The y to be controlled
	 * @return true if and only if y is positive and y is smaller then the board limit.
	 * 		| result == (y >= 0 && y <= getYLimit())
	 */
	@Raw
	public boolean canHaveAsY(long y) {
		return (y >= 0 && y <= getYLimit());
	}
	
	/**
	 * 
	 * returns the maximum y-value (the edge of the board), Long.MAX_VALUE.
	 */
	@Basic @Raw @Immutable
	public static long getYLimit(){
		return Y_BOARD_LIMIT;
	}
	
	//The y position of the robot.
	private long y;
	
	//The y position limit of the robot.
	private static final long Y_BOARD_LIMIT  = Long.MAX_VALUE;
	
//energy: nominally -------------------------------------------------------------------------------------------
	
	/**
	 * Returns the energy level of this robot
	 * 
	 */
	@Basic @Raw
	public double getEnergyLevel() {
		return this.energyLevel;
	}
	
	/**
	 * Return the energy level of this robot as a fraction of the maximum amount
	 * 
	 * @return a number between 0 and 1 indicating the energy level as a fraction of the maximum amount
	 * 		| result == this.getEnergyLevel() / this.getEnergyLimit()
	 */
	@Raw
	public double getFractialEnergyLevel() {
		return getEnergyLevel() / getEnergyLimit();
	}
	

	/**
	 * Calculates the minimum energy required to reach a given position.
	 * 
	 * @param xDestination The x-coordinate of the position to be reached.
	 * @param yDestination The y-coordinate of the position to be reached.
	 * @pre The x-coordinate of the position must be valid.
	 * 		| canHaveAsX(xDestination)
	 * @pre The y-coordinate of the position must be valid.
	 * 		| canHaveAsY(yDestination)
	 * @return The minimum amount of energy to reach the given position, with turning in two directions.
	 * 		| if(this.getX()==xDestination ||  this.getY() == yDestination))
	 * 		The position of the robot and the destination position are on one line. The robot must only turn to the right orientation to reach the position.
	 * 		| 	then result == ENERGY_ROTATION * Math.abs(turnsBeforeMovement(xDestination, yDestination)) + 
	 * 		|			       ENERGY_MOVEMENT*(Math.abs(xDestination-this.getX()) + Math.abs(yDestination-this.getY()))
	 * 		The position of the robot and the destination position are not on one line. The robot must turn to the right orientation to start moving and must turn
	 * 		one time more to move in a L to the given position.
	 * 		| 	else result == ENERGY_ROTATION * (Math.abs(turnsBeforeMovement(xDestination, yDestination)) + 1) + 
	 * 		|			       ENERGY_MOVEMENT*(Math.abs(xDestination-this.getX()) + Math.abs(yDestination-this.getY()))
	 */
	public double getEneryRequiredToReach(long xDestination, long yDestination){
		assert(canHaveAsX(xDestination) && canHaveAsY(yDestination));
		int amountOfTurns = Math.abs(turnsBeforeMovement(xDestination, yDestination));
		if(!(this.getX()==xDestination ||  this.getY() == yDestination)) // They are not standing on one line, so he must turn, then move, then they are on one line
			//																, and then turn again one time, and move again
			amountOfTurns++;
		
		return ENERGY_MOVEMENT*(Math.abs(xDestination-this.getX()) + Math.abs(yDestination-this.getY()))
				+ ENERGY_ROTATION*(amountOfTurns);	
	}
	
	/**
	 * Sets the energyLevel of this robot to the given value.
	 * 
	 * @param energyLevel the energy level that the robot will get.
	 * @pre	The value of energyLevel must be valid.
	 * 		| canHaveAsEnergyLevel(energyLevel)
	 * @post The energy level of the robot is equal to the given value.
	 * 		| (this new).getEnergyLevel == energyLevel
	 */
	@Raw
	public void setEnergyLevel(double energyLevel) {
		assert(canHaveAsEnergyLevel(energyLevel));
		this.energyLevel = energyLevel;
	}
	
	/**
	 * Increase the energy level of this robot by the given amount.
	 * 
	 * @param energy The amount to be added.
	 * @pre The amount must be positive.
	 * 		| energy >= 0
	 * @pre The robot may not be terminated.
	 * 		| !isTerminated()
	 * @effect has the same result as setting the energy level to the current energy level plus the 
	 * 			amount indicated by energy if allowed.
	 * 		| setEnergyLevel(getEnergyLevel() + energy)
	 */
	public void recharge(double energy) {
		assert(energy>=0);
		assert(!isTerminated());
		energy = getEnergyLevel() + energy;
		setEnergyLevel(energy); // 'inherits' the precondition that getEnergyLevel() + energy must be a legal value.
	}
	
	/**
	 * Controls whether the energy level is valid.
	 * 
	 * @param energyLevel the energy level to be controlled
	 * @return	true if and only if the energy level is valid.
	 * 		| result == isPossibleEnergyLevel(energyLevel) && energyLevel <= getEnergyLimit())
	 */
	@Raw
	public boolean canHaveAsEnergyLevel(double energyLevel) {
		return (isPossibleEnergyLevel(energyLevel) && energyLevel <= getEnergyLimit()); // second thing is specific for this robot, so there are two methods.
	}
	
	/**
	 * Controls if the given energyLevel is possible.
	 * 
	 * @param energyLevel The energyLevel to be controlled.
	 * @return true if the energyLevel is possible.
	 * 		| result == energyLevel >= 0
	 */
	public static boolean isPossibleEnergyLevel(double energyLevel) { // for all robots, so an different method
		return energyLevel >= 0;
	}
	
	/**
	 * Decreases the energy level by the given amount
	 * 
	 * @param energy the energy amount to subtract.
	 * @pre	the amount must be positive.
	 * 		| energy >= 0
	 * @pre The robot may not be terminated.
	 * 		| !isTerminated()
	 * @effect has the same result as setting the energy level to the current energy level minus the amount indicated by energy.
	 * 		| setEnergyLevel(getEnergyLevel() - energy)
	 */
	@Raw
	public void consumeEnergy(double energy) {
		assert(energy>=0);
		assert(!isTerminated());
		energy = getEnergyLevel() - energy;
		setEnergyLevel(energy); // 'inherits' the precondition that getEnergyLevel() - energy must be a legal valid.
	}
	
	//The energy level of the robot.
	private double energyLevel;
	
	/**
	 * returns the maximum amount of energy.
	 * 
	 */
	@Basic @Raw @Immutable
	public double getEnergyLimit() {
		return this.ENERGY_LIMIT;
	}
	
	//The energy limit of the robot.
	private final double ENERGY_LIMIT;
	
	/**
	 * The default energy limit of the robot.
	 */
	public final static double DEFAULT_ENERGY_LIMIT = 20000D;
	
// Turning: total ----------------------------------------------------------------------------------------------------
	/**
	 * Turns the robot to right or left according to the argument, a positive number will turn clockwise, a negative counterclockwise.
	 * 
	 * @param clockwise The orientation to be turned to, positive turns clockwise, negative to the left, zero doesn't turn at all.
	 * @effect Turns the robot according to the sign of clockwise if the robot has enough energy to do so.
	 * 		| if(!isTerminated() && getEnergyLevel() >= Robot.ENERGY_ROTATION)
	 * 		|	then setOrientation((getOrientation + (int) Math.signum(clockwise) + 4) % 4)
	 * @effect Consumes an amount of energy if the robot has enough energy to turn, and if clockwise is not zero.
	 * 		| if(!isTerminated() && getEnergyLevel() >= Robot.ENERGY_ROTATION)
	 * 		| 	if (Math.signum(clockwise) != 0)
	 * 		|		then consumeEnergy(Robot.ENERGY_ROTATION)
	 */
	public void turn(int clockwise) {
		if(!isTerminated() && getEnergyLevel() >= ENERGY_ROTATION) {
			clockwise = (int) Math.signum(clockwise);
			if(clockwise != 0) {
				setOrientation((getOrientation() + clockwise + 4) % 4); // the +4)%4 is added because getOrientation()+clockwise could be -1 (looking up and tuning left)
				consumeEnergy(ENERGY_ROTATION); // preconditions of consumeEnergy are checked.
			}
		}
	}
	
	/**
	 * Turns the robot some steps, while consuming the needed energy, to the right or left depending on the sign of clockwise,
	 * if he has the required energy level.
	 * 
	 * @param clockwise The orientation to be turned to, positive turns clockwise, negative to the left, zero doesn't turn at all.
	 * @param steps The number of times the robot must turn 90 degrees.
	 * @pre The robot must have sufficient energy.
	 * 		| Math.abs(steps) * ENERGY_ROTATION <= this.getEnergyLevel()
	 * @effect Turns the robot.
	 * 		| if steps == Integer.MIN_VALUE
	 * 		|	then for each i in {x| x> 0 ; x <= Integer.MAX_VALUE}
	 * 		|			turn(clockwise)
	 * 		| else 
	 * 		|	then for each i in {x| x> 0; x <= Math.abs(steps)}
	 * 		|			turn(clockwise)
	 */
	public void turn(int clockwise, int steps) {
		if(Math.signum(clockwise) != 0) {
			int i = 0;
			if(steps == Integer.MIN_VALUE) 
				steps = Integer.MAX_VALUE;
			else 
				steps = Math.abs(steps);
			assert Math.abs(steps) * ENERGY_ROTATION <= this.getEnergyLevel();
			while( i < steps) {
				turn(clockwise);
				i++;
			}
		}
	}
		
	
	/**
	 * The energy needed to turn.
	 */
	public static final double ENERGY_ROTATION = 100D;
	
	/**
	 * The value for turning clockwise (every positive number different from zero will do the job)
	 */
	public static final int CLOCKWISE = 1;
	
	/**
	 * The value for turning counterclockwise (every negative number will do the job)
	 */
	public static final int COUNTERCLOCKWISE = -1;
	
//Moving defensive -----------------------------------------------------------------------------------------------------
	
	/**
	 * Moves the robot one time in the direction of its orientation if possible. 
	 * 
	 * @effect If there is enough energy to move, the robot will consume energy while doing so.
	 * 		|	consumeEnergy(Robot.ENERGY_MOVEMENT)
	 * @effect If there is enough energy for a movement, then by moving the robot will get a new x- or y-position, depending on its orientation:
	 * 			when facing up(down), the new y value will be one less(more) if possible without leaving the board.
	 * 			when facing right(left), the new x value will be one more(less) if possible without leaving the board.
	 * 		| 	if (getOrientation()==UP) setY(getY() - 1)
	 * 		| 	if (getOrientation()==RIGHT) setX(getX() + 1)
	 * 		| 	if (getOrientation()==DOWN) setY(getY() + 1)
	 * 		| 	if (getOrientation()==LEFT) setX(getX() - 1)
	 * @throws RuntimeException
	 * 		Happens when the robot doesn't satisfy its class invariants that the orientation must be valid.
	 * 		| !canHaveAsOrientation(this.getOrientation())
	 * @throws IllegalStateException
	 * 		The robot is terminated and can't be moved.
	 * 		| isTerminated()
	 * @throws IllegalArgumentException
	 * 		If the robot will leave the board.
	 * 		| !canHaveAsX((new this).getX()) || !canHaveAsY((new this).getY())
	 * 
	 */
	public void move() throws RuntimeException, IllegalArgumentException,
	IllegalStateException {
		if(!isTerminated()) {
			long tempX = this.getX();
			long tempY = this.getY();
			switch(this.getOrientation()) {
				case UP: setY(tempY - 1);
						break;
				case RIGHT: setX(tempX + 1);
						break;
				case DOWN: setY(tempY + 1);
						break;
				case LEFT: setX(tempX - 1);
						break;
				default: throw new RuntimeException(); //should not happen
			}
			consumeEnergy(ENERGY_MOVEMENT); // inherits the precondition that getEnergyLevel-ENERGY_MOVEMENT must be a valid value.
		}
		else
			throw new IllegalStateException("The robot is terminated");
	}
	
	/**
	 * Moves this robot and the other as close as possible to each other (they will end up with minimal Manhattan distance) given their energy level.
	 * 
	 * @param robot The robot next to which this robot must end, if possible.
	 * @pre The robot must be a real robot.
	 * 		| robot != null
	 * @effect Moves the robots apart if they are together and if possible.
	 * 		| if this.together(robot)
	 * 		| 	then this.moveFromEachOther(robot)
	 * @post For every possible position in the rectangle determined by the position of this and the other robot, if they can reach a combination of positions and are not
	 * 		together but next to each other, then the total needed energy for both is greater or equal than the energy required to move to their new positions. Otherwise,
	 * 		when they can reach the combination of positions but are not together and not next to each other, then the Manhattan distance between them is greater or equal
	 * 		to the Manhattan distance after the movements executed by this method.
	 * 		| if !this.together(robot)
	 * 		| 	then let d((x1,y1),(x2,y2)) stand for the Manhattan distance between a position with x = x1 and y = y1, and a position with x = x2 and y = y2.
	 * 		|        for each (x, y) in {(x,y) | x is in [this.getX(), robot.getX()] && y is in [this.getY(), robot.getY()]}
	 * 		|	     for each (xRobot, yRobot) in {(x,y) | x is in [this.getX(), robot.getX()] && y is in [this.getY(), robot.getY()]}
	 * 		|		 for each thisOrientation in {Robot.UP, Robot.RIGHT, Robot.DOWN, Robot.LEFT}
	 * 		|		 for each robotOrientation in {Robot.UP, Robot.RIGHT, Robot.DOWN, Robot.LEFT}	
	 * 		|		    if (this old).getEnergyRequiredToReach(x,y) <= (this old).getEnergyLevel() && (robot old).getEnergyRequiredToReach(xRobot, yRobot) <= (robot old).getEnergyLevel() && !this.together(robot)
	 * 		|			   then let thisClone = new Robot(x,y,thisOrientation,(this old).getEnergyLevel(), (this old).getEnergyLimit())
	 * 		|					let robotClone = new Robot(x,y,robotOrientation,(this robot).getEnergyLevel(), (this robot).getEnergyLimit())
	 * 		|					if thisClone.nextTo(robotClone)
	 * 		|				       then ( (this old).getEnergyRequiredToReach((this new).getX(), (this new).getY()) + (robot old).getEnergyRequiredToReach((robot new).getX(), (robot new).getY()) )
	 * 		|						    <= ( (this old).getEnergyRequiredToReach(x, y) + (robot old).getEnergyRequiredToReach(xRobot, yRobot) )
	 * 		| 			        else
	 * 		|				      then d( ((this new).getX(), (this new).getY()), ((robot new).getX(), (robot new).getY()) ) <= d( (x,y),(xRobot, yRobot))
	 * @throws IllegalStateException
	 * 			When this or the robot is terminated.
	 * 		| if this.isTerminated() || robot.isTerminated()
	 */
	public void moveNextTo(Robot robot) {
		assert (robot != null);
		if (!this.isTerminated() && !robot.isTerminated()) {
			if (this != robot) {
				if (!this.together(robot)) {
					if (!onOneLine(robot)) // are the robots on one line?
						this.moveBothToOneLine(robot); // no, than try to move
														// then on one line.
					if (onOneLine(robot)) // if we can move them to one line,
											// they are now on one line and this
											// must be executed. 'Else' can not
											// be used.
						this.moveBothFromOneLine(robot); // they are on one
															// line, so lets try
															// to move them next
															// to each other.
				} else
					this.moveFromEachOther(robot);
			}
		} else
			throw new IllegalStateException("The robot is terminated");
	}
	
	/**
	 * Tries to move as efficiently as possible the robots from each other, assuming they are together
	 * 
	 * @param robot The robot on the same position as this.
	 * @pre The robot must be a real robot.
	 * 		| robot != null
	 * @pre This and the robot must have the same position
	 * 		| this.together(robot)
	 * @effect Moves the robots apart if possible. They need the energy and must reach a valid position.
	 * 		| let turnsThis = this.neededTurnsLegalMove()
	 * 		| let turnsRobot = robot.neededTurnsLegalMove()
	 * 		| if Math.abs(turnsThis) <= Math.abs(turnsRobot)
	 * 		|	then this.tryThisMoveFirstFromEachOther(this, turnsThis, turnsRobot)
	 * 		| else
	 * 		|	robot.tryThisMoveFirstFromEachOther(robot, turnsRobot, turnsThis
	 */
	@Model
	private void moveFromEachOther(Robot robot) {
		assert this.together(robot);
		assert robot != null;
		int turnsThis = this.neededTurnsLegalMove();
		int turnsRobot = robot.neededTurnsLegalMove();
		if(Math.abs(turnsThis) <= Math.abs(turnsRobot))
			this.tryThisMoveFirstFromEachOther(robot, turnsThis, turnsRobot);
		else
			robot.tryThisMoveFirstFromEachOther(this, turnsRobot, turnsThis);
	}
	
	/**
	 * Moves this away from the robot, if this has the required energy. Otherwise the method tries to move robot away from this if he has the required energy.
	 * 
	 * @param robot The robot to move from.
	 * @param turnsThis The turns needed for this to move to a possible orientation next to the current one.
	 * @param turnsRobot The turns needed for robot to move to a possible orientation next to the current one.
	 * @pre This and the robot must have the same position.
	 * 		| this.together(robot)
	 * @pre The robot must be a real robot.
	 * 		| robot != null
	 * @effect if this.getEnergyLevel() >=  ENERGY_MOVEMENT + ENERGY_ROTATION * Math.abs(turnsThis)
	 * 		|	then this.turn(turnsThis, turnsThis) 
	 * 		|		 this.move()
	 * 		|	else if robot.getEnergyLevel() >=  ENERGY_MOVEMENT + ENERGY_ROTATION * Math.abs(turnsRobot)
	 * 		|			then robot.turn(turnsRobot, turnsRobot)
	 * 		|				 robot.move()
	 */
	@Model
	private void tryThisMoveFirstFromEachOther(Robot robot, int turnsThis, int turnsRobot) {
		assert this.together(robot);
		assert robot != null;
		if(this.getEnergyLevel() >=  ENERGY_MOVEMENT + ENERGY_ROTATION * Math.abs(turnsThis) ) {
			this.turn(turnsThis, turnsThis);
			this.move();
		}
		else if(robot.getEnergyLevel() >=  ENERGY_MOVEMENT + ENERGY_ROTATION * Math.abs(turnsRobot) ) {
			robot.turn(turnsRobot, turnsRobot);
			robot.move();
		}
	}
	
	/**
	 * Returns the minimum turns needed to move to a possible position next to the current position.
	 *
	 * @return The minimum turns needed to move to any legal position next to the robot.
	 * 		| let minimum = 1, let negative = false
	 * 		| for each (x,y) in {(this.getX() + 1, this.getY()), (this.getX() - 1, this.getY()), (this.getX(), this.getY() + 1)), (this.getX(), this.getY() - 1)) }
	 * 		|	do if canHaveAsX(x) && canHaveAsY(y)
	 * 		|		then if Math.abs(turnsBeforeMovement(x, y) <= min
	 * 		|			then minimum = Math.abs(turnsBeforeMovement(x, y)
	 * 		|				if turnsBeforeMovenement(x,y) < 0
	 * 		|					then negative = true
	 * 		| if(negative)
	 * 		|	then result == - minimum
	 * 		| else
	 * 		|	then result == minimum
	 */
	@Model
	private int neededTurnsLegalMove() {
		Set<Integer> neededTurns = new HashSet<Integer>();
		if(canHaveAsX(this.getX() + 1))
			neededTurns.add(turnsBeforeMovement(this.getX() + 1, this.getY()));
		if(canHaveAsX(this.getX() - 1))
			neededTurns.add(turnsBeforeMovement(this.getX() - 1, this.getY()));
		if(canHaveAsY(this.getY() + 1))
			neededTurns.add(turnsBeforeMovement(this.getX(), this.getY() + 1));
		if(canHaveAsY(this.getY() - 1))
			neededTurns.add(turnsBeforeMovement(this.getX(), this.getY() - 1));
		if(neededTurns.contains(0))
			return 0;
		else if(neededTurns.contains(1))
			return 1;
		else 
			return -1;
	}
	
	/**
	 * Tries to move the robots so they end up on one line. If not possible, they will be moved as close to each other as possible.
	 * 
	 * @param robot The robot to be moved.
	 */
	private void moveBothToOneLine(Robot robot) {
		assert(!onOneLine(robot));
		int turnsThis = this.turnsBeforeMovement(robot.getX(), robot.getY());
		int turnsRobot = robot.turnsBeforeMovement(this.getX(), this.getY());
		if(Math.abs(turnsThis) <= Math.abs(turnsRobot))
			this.tryMoveToOneLine(turnsThis, robot);
		if(!onOneLine(robot))//when they are still on one line, execute the following line.
			robot.tryMoveToOneLine(turnsRobot, this);	
		if(!onOneLine(robot)){//when they are still not one line, try to move them to one line by moving both if they have the right orientation. 
			int temp = Math.abs(Math.abs((this.getOrientation() + turnsThis + 4)%4) - Math.abs((robot.getOrientation() + turnsRobot + 4)%4));
			// if temp == 2; then after the turnsThis and the turnsRobot, they are facing opposite directions (one up and other down for instance) and towards eachother.
			if(temp==2){ // now we can try to move them to one line because they are facing the right directions to do so
						 // for example: one on (0,0) looking down and one on (3,5) looking up, so we try to move them as close as possible to line (the same y-coordinate)
				this.moveAsCloseAsToOneLine(turnsThis, robot);
				robot.moveAsCloseAsToOneLine(turnsRobot, this);
			}
			else { // they are both looking towards the same point (for example: if one on (0,0) looking right and one on (10,10) looking up, they will look to (0,10)).
				   // So we try to move them as close as possible to the point where they are looking to, (10,0) in the example.
				this.moveAsCloseAs(turnsThis, robot);
				robot.moveAsCloseAs(turnsRobot, this);
			}
		}
	}
	
	/**
	 * Tries to move this robot so he ends up on one line with the other.
	 * @param turnsThis The number of turns this robot must do before he can move in the right direction.
	 * @param robot The robot to move towards.
	 */
	private void tryMoveToOneLine(int turnsThis,
			Robot robot) {
		assert(!onOneLine(robot));
		Robot clone = this.clone();
		this.moveAsCloseAsToOneLine(turnsThis, robot);
		if(!this.onOneLine(robot))
			this.reset(clone);
	}
	
	/**
	 * If the robot's don't have enough energy to end up on one line, this method will move them as close as possible to one line and thus reduce the Manhattan distance.
	 * 
	 * @param turnsThis The number of turns this robot must do before he can move in the right direction.
	 * @param robot The robot to move towards.
	 */
	private void moveAsCloseAsToOneLine(int turnsThis, Robot robot){
		assert(!onOneLine(robot));
		if(Math.abs(turnsThis) * ENERGY_ROTATION + ENERGY_MOVEMENT <= this.getEnergyLevel()) {
			this.turn(turnsThis, Math.abs(turnsThis));
			while(!this.onOneLine(robot) && ENERGY_MOVEMENT <= this.getEnergyLevel())
				this.move();
		}
	}
	
	/**
	 * Tries to move the robots next to each other once they are standing on one line. If not possible, they will end up as close as possible to each other on one line.
	 * 
	 * @param robot The robot to move towards.
	 */
	private void moveBothFromOneLine(Robot robot) {
		if(!nextTo(robot)){
			assert(onOneLine(robot));
			int turnsThis = this.turnsBeforeMovement(robot.getX(), robot.getY());
			int turnsRobot = robot.turnsBeforeMovement(this.getX(), this.getY());
			if(Math.abs(turnsThis) <= Math.abs(turnsRobot))
				this.tryMoveTogetherFromOneLine(turnsThis, robot);
			if(!this.nextTo(robot)) // The first robot (this) can't reach the other (robot) so we try to move the other (robot) next to the first (this)
				robot.tryMoveTogetherFromOneLine(turnsRobot, this);
			if(!this.nextTo(robot)) { // The other (robot) also can't reach the first (this) so we try to move them both as close as possible to each other on one line.
				this.moveAsCloseAs(turnsThis,robot);
				robot.moveAsCloseAs(turnsRobot,this);
			}
		}
	}
	
	/**
	 * Tries to move this robot next to the other once they are standing on one line.
	 * 
	 * @param turnsThis The number of turns this robot must do before he can move in the right direction.
	 * @param robot The robot to move towards.
	 */
	private void tryMoveTogetherFromOneLine(int turnsThis,
			Robot robot) {
		assert(onOneLine(robot));
		Robot clone = this.clone();
		this.moveAsCloseAs(turnsThis, robot);
		if(!this.nextTo(robot))
			this.reset(clone);
	}
	
	/**
	 * Moves the robots as close to each other as possible, they don't have to stand on one line.
	 * 
	 * If they are not one line: 	can only be evoked if they are not looking in opposite directions!! (see moveBothToOneLine() for move explanation)
	 * 								then they will move as close to an common point as possible. (see moveBothToOneLine() for move explanation)
	 * If they are on one line:	if possible they will end up next to each other, if not possible they will end up as close as possible to each other on one line.
	 * 
	 * @param turnsThis The number of turns this robot must do before he can move in the right direction.
	 * @param robot The robot to move towards.
	 */
	private void moveAsCloseAs(int turnsThis, Robot robot) {
		if(Math.abs(turnsThis) * ENERGY_ROTATION + ENERGY_MOVEMENT <= this.getEnergyLevel()) {
			this.turn(turnsThis, Math.abs(turnsThis));
			while(!this.nextTo(robot) && ENERGY_MOVEMENT <= this.getEnergyLevel())
				this.move();
		}	
	}
	
	/**
	 * Controls whether this robot and the other are standing on one line.
	 * 
	 * @param robot The robot to control.
	 * @pre The robot must be a real robot.
	 * 		| robot != null
	 * @return True if this and the given robot are on one line, otherwise false.
	 * 		| result == (this.getX() == robot.getX() || this.getY() == robot.getY())
	 */
	private boolean onOneLine(Robot robot) {
		assert robot != null;
		return this.getX() == robot.getX() || this.getY() == robot.getY();
	}
	
	/**
	 * Controls whether this robot and the other are standing next to each other.
	 * 
	 * @param robot The robot to compare with.
	 * @pre The robot must be a real robot.
	 * 		| robot != null
	 * @return True if this robot and the other are standing next to each other.
	 * 		| result == ( 1 == (Math.abs(this.getX()-robot.getX()) + Math.abs(this.getY()-robot.getY())) )
	 */
	@Model
	private boolean nextTo(Robot robot){
		assert(robot!= null);
		return 1==(Math.abs(this.getX()-robot.getX()) + Math.abs(this.getY()-robot.getY()));				
	}
	
	/**
	 * Controls if this and the robot are on the same position.
	 * 
	 * @param robot The robot to compare with.
	 * @pre The robot must be a real robot
	 * 		| robot != null
	 * @return True if the robots are on the same position otherwise false.
	 * 		| result == (this.getX() == robot.getX() && this.getY() == robot.getY())
	 */
	@Model
	private boolean together(Robot robot) {
		assert(robot!= null);
		return this.getX() == robot.getX() && this.getY() == robot.getY();
	}
	
	/**
	 * Makes a copy of the robot.
	 * 
	 * @return A copy of this.
	 * 		| result == this(this.getX(), this.getY(), this.getOrientation(), this.getEnergyLevel(), this.getEnergyLimit())
	 */
	public Robot clone() {
		return new Robot(this.getX(), this.getY(), this.getOrientation(), this.getEnergyLevel(), this.getEnergyLimit());
	}
	
	/**
	 * The energy needed to move.
	 */
	public static final double ENERGY_MOVEMENT = 500D;
	
	/**
	 * Calculates the turns needed before you can move efficiently to reach a given position.
	 *
	 * @param xDestination The x-coordinate of the position to be reached.
	 * @param yDestination The y-coordinate of the position to be reached.
	 * @pre The x-coordinate of the position must be valid.
	 * 		| canHaveAsX(xDestination)
	 * @pre The y-coordinate of the position must be valid.
	 * 		| canHaveAsY(yDestination)
	 * @return If the robot is looking in a good direction, the result will be zero (the robot doen't have to turn before the robot has to move to the given position)
	 * 			If the robot has to turn at least one time clockwise in order to look in a good direction, the result will be one.
	 * 			If the robot has to turn at least one time counterclockwise in order to look in a good direction, the result will be minus one.
	 * 			If the robot has to turn at least two times in order to look in a good direction, the result will be two.
	 * 		| if ( (this.getOrientation() == UP && yDestination<this.getY()) || (this.getOrientation() == RIGHT && xDestination>this.getX())
	 * 		|		|| (this.getOrientation() == DOWN && yDestination>this.getY()) || (this.getOrientation() == LEFT && xDestination<this.getX())
	 * 		|		|| this.getX()==xDestination && this.getY() == yDestination)
	 * 		|	then result == 0
	 * 		| else if ( (this.getOrientation() == UP && xDestination>this.getX()) || (this.getOrientation() == RIGHT && yDestination>this.getY())
	 * 		|		|| (this.getOrientation() == DOWN && xDestination<this.getX()) || (this.getOrientation() == LEFT && yDestination<this.getY()) )
	 * 		|	then result == 1
	 *  	| else if ( (this.getOrientation() == UP && xDestination<this.getX()) || (this.getOrientation() == RIGHT && yDestination<this.getY())
	 * 		|		|| (this.getOrientation() == DOWN && xDestination>this.getX()) || (this.getOrientation() == LEFT && yDestination>this.getY()) )
	 * 		|	then result == -1
	 * 		| else if ( (this.getOrientation() == UP && yDestination>this.getY()) || (this.getOrientation() == RIGHT && xDestination<this.getX())
	 * 		|		|| (this.getOrientation() == DOWN && yDestination<this.getY()) || (this.getOrientation() == LEFT && xDestination>this.getX()) )
	 * 		|	then result == 2
	 */
	@Model
	private int turnsBeforeMovement(long xDestination, long yDestination){
		assert(canHaveAsX(xDestination) && canHaveAsY(yDestination));
		Set<Integer> movementPossibilities = new HashSet<Integer>();
		if(this.getX()==xDestination && this.getY() == yDestination)
			return 0;
		if(xDestination-this.getX()>0)
			movementPossibilities.add(RIGHT);
		if(xDestination-this.getX()<0)
			movementPossibilities.add(LEFT);
		if(yDestination-this.getY()>0)
			movementPossibilities.add(DOWN);
		if(yDestination-this.getY()<0)
			movementPossibilities.add(UP);
		
		int step = 0; // the order to control: 0,1,-1,2
		boolean found = false;
		while(step < 3 && !found) {
			if(movementPossibilities.contains((this.getOrientation() + step +4)%4))
				found = true;
			else {
				step++;
				if(step == 2) step = -1; // afther 1, then comes -1
				else if(step == 0) step = 2; // afther -1 then comes 2
			}				
		}
		return step;
	}
	
	/**
	 * Resets this to the values of the given robot.
	 * 
	 * @param robot The robot needed to set this back as.
	 * @pre The robot must be a real robot
	 * 		| robot != null
	 * @effect Sets the x-coordinate of this to the value of the given robot.
	 * 		| this.setX(robot.getX())
	 * @effect Set the y-coordinate of this to the value of the given robot.
	 * 		| this.setY(robot.getY())
	 * @effect Set the orientation of this to the value of the given robot.
	 * 		| this.setOrientation(robot.getOrientation())
	 * @effect Set the energy level of this to the value of the given robot.
	 * 		| this.setEnergyLevel(robot.getEnergyLevel())
	 */
	private void reset(Robot robot) {
		assert(robot != null);
		this.setX(robot.getX());
		this.setY(robot.getY());
		this.setOrientation(robot.getOrientation());
		this.setEnergyLevel(robot.getEnergyLevel());
	}
	

	
	/**
	 * Is equal to getEnergyRequiredToReach, but allows only clockwise turning.
	 * So it returns the minimum amount of energy to reach a given position when only turning clockwise is allowed.
	 * 
	 * @param x
	 * @param y
	 * @return
	 * @throws IllegalArgumentException
	 */
/*	@Deprecated
	private double getEneryRequiredToReachClockwise(long x, long y) throws IllegalArgumentException {
		if(!canHaveAsX(x) || !canHaveAsY(y))
			throw new IllegalArgumentException();
		return recGetEneryRequiredToReach(getOrientation(), getX(), getY(), x, y, 0);
	}
	*/
	
	/**
	 * Calculates the minimum amount of energy to reach a given position when only turning clockwise is allowed with a recursive algorithm.
	 * 
	 * @pre the values must be logical 
	 * 		|canHaveAsX(x1) && canHaveAsX(x2) && canHaveAsY(y1) && canHaveAsY(y2)
	 * @param orientation The current orientation of the robot.
	 * @param x1 The x coordinate of the start position.
	 * @param y1 The y coordinate of the start position.
	 * @param x2 The x coordinate of the end position.
	 * @param y2 The y coordinate of the end position.
	 * @param usedEnergy The energy used so far.
	 * @return
	 * @throws IllegalArgumentException
	 * 		If the orientation is invalid, valid values are 0,1,2 and 3.
	 * 		| orientation < 0 || orientation > 4
	 * 		
	 */
/*	@Deprecated
	private double recGetEneryRequiredToReach(int orientation, long x1, long y1, long x2, long y2, double usedEnergy) 
			throws IllegalArgumentException{
//		 First, the orientation is checked, then when we know the orientation, it is checked whether this is a good direction to move forward.
//		 If it is, the robot will move as far in that direction as he needs to do, and the algorithm will be executed again.
//		 If it isn't, the robot will turn clockwise one time and then the algorithm will be executed again.
//		 
//		 This continues until the destination is reached, then the amount of energy needed to get here is returned.
		if((x1==x2) && (y1==y2))
			return usedEnergy;
	
		switch(orientation) {
			case UP: 
				if(y2<y1) //move up to y2
					return recGetEneryRequiredToReach(UP, x1, y2, x2, y2, (usedEnergy+ENERGY_MOVEMENT*(y1-y2)));
				else 
					return recGetEneryRequiredToReach((orientation+1)%4, x1, y1, x2, y2, (usedEnergy+ENERGY_ROTATION));
			case RIGHT: 
				if(x2>x1) //move to the right to x2
					return recGetEneryRequiredToReach(RIGHT, x2, y1, x2, y2, (usedEnergy+ENERGY_MOVEMENT*(x2-x1)));
				else
					return recGetEneryRequiredToReach((orientation+1)%4, x1, y1, x2, y2, (usedEnergy+ENERGY_ROTATION));
			case DOWN: 
				if(y2>y1) //move down to y2
					return recGetEneryRequiredToReach(DOWN, x1, y2, x2, y2, (usedEnergy+ENERGY_MOVEMENT*(y2-y1)));
				else 
					return recGetEneryRequiredToReach((orientation+1)%4, x1, y1, x2, y2, (usedEnergy+ENERGY_ROTATION));	
			case LEFT: 
				if(x2<x1) //move to the left to x2
					return recGetEneryRequiredToReach(LEFT, x2, y1, x2, y2, (usedEnergy+ENERGY_MOVEMENT*(x1-x2)));
				else 
					return recGetEneryRequiredToReach((orientation+1)%4, x1, y1, x2, y2, (usedEnergy+ENERGY_ROTATION));
			default: throw new IllegalArgumentException(); //should not happen: orientation is invalid
		}
	
	}
	*/
}



